#ifndef DRIVABLE_SEG_H_
#define DRIVABLE_SEG_H_

#include <ros/ros.h>
#include <opencv/cv.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/common/common.h>
#include <opencv2/imgproc.hpp>
#include "glog/logging.h"
#include <vector>
#include <cmath>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/PointCloud2.h>
//#include <Depth/DrivableSegUtility.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/segmentation/extract_clusters.h>
#include <GroundPlaneFit.h>
//    API 1:ComputeDrivableArea(const cv::Mat &range_mat,pcl::PointCloud<PointType>::Ptr InputfullCloud,
//                              pcl::PointCloud<PointType>::Ptr ground_cloud,cv::Mat &ground_mat)
//    输入深度图和其对应的点云，输出地面点云，地面点的标记(仅适用于LeGO-LOAM)
//    API 2:ComputeDrivableArea(const cv::Mat &range_mat,pcl::PointCloud<PointType>::Ptr ground_cloud)
//    输入深度图，深度图中没有深度的点为FLT_MAX，输出地面点云
//    API 3:ComputeDrivableArea(pcl::PointCloud<PointType>::ConstPtr laserCloudIn,pcl::PointCloud<PointType>::Ptr ground_cloud)
//    输入原始点云，输出地面点云
//
//    cv::Mat GetGroundMat()获取地面标记
//    cv::Mat GetAngleImage();获取角度图
//    cv::Mat GetRangeMat();获取深度图
using namespace std;
class DrivableSeg {

    typedef pcl::PointXYZI PointType;

//    struct PointXYZIR {
//        PCL_ADD_POINT4D
//
//        PCL_ADD_INTENSITY;
//        uint16_t ring;
//
//        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//    } EIGEN_ALIGN16;

public:
    DrivableSeg(ros::NodeHandle &nh);

    ~DrivableSeg() {};

    void ComputeScanAngle(const cv::Mat &range_mat);

    void ComputeRangeMat(const pcl::PointCloud<PointType> InputCloud, cv::Mat &range_mat);

    void ComputeDrivableArea(pcl::PointCloud<pcl::PointXYZ>::ConstPtr laserCloudIn,
                             pcl::PointCloud<pcl::PointXYZI> &segmentedCloud,
                             const int clusterpurpose,
                             const int MinClusterNub,
                             bool &is_stair_flag);

    void extract_object();

    void labelComponents(int row, int col);

    void merge_object();

    void ComputePlaneParam(pcl::PointCloud<pcl::PointXYZI>::Ptr &PlaneCloud,
                           bool &is_stair_flag);

private:

    vector<float> row_angle_sin;
    vector<float> row_angle_cos;

    cv::Mat angle_image;
    cv::Mat x_mat;
    cv::Mat y_mat;
    cv::Mat rangeMat;
    cv::Mat groundMat;
    cv::Mat labelMat;

    PointType nanPoint;

    pcl::PointCloud<PointType>::Ptr fullPointCloud;
    pcl::PointCloud<PointType>::Ptr SegObjectCloud;
    pcl::PointCloud<PointType>::Ptr GroundCloud;
    pcl::PointCloud<pcl::PointXYZ>::Ptr ClusteredCloudCenter;

    std::vector<std::pair<int8_t, int8_t> > neighborIterator; // neighbor iterator for segmentaiton process
    std::vector<int> clusteredcloudstart;
    std::vector<int> clusteredcloudnum;
    std::vector<int> clusteredcloudlabel;

    uint16_t *allPushedIndX; // array for tracking points of a segmented object
    uint16_t *allPushedIndY;

    uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed
    uint16_t *queueIndY;

    int clusterpurpose;
    shared_ptr<GroundPlaneFit> gpfmethod;

    float dx, dy;

    int labelCount;
    int MinClusterNub;
    int maxclusteredlabel;
    int maxclusterednub;

    //雷达投影参数
    int N_SCAN;
    int Horizon_SCAN;
    double ang_res_x;
    double ang_res_y;
    double ang_bottom;
    double vertical_bottom;
    double horizon_bottom;
    double CAR_HEIGHT;
    double sensorMinimumRange;

    //障碍物聚类参数
    double segmentAlphaX;
    double segmentAlphaY;
    double segmentTheta;
    int segmentValidPointNum;
    int segmentValidLineNum;

    double toler; // angle threshold of two points
    int min_size;  // The data of KITTI is not the original lidar signal, we need to consider those holes when search for nearest point in four directions.
    int max_size;

};


#endif
